package com.yuneec.droneservice.command;

import com.yuneec.droneservice.model.JoystickValues;
import com.yuneec.droneservice.request.IResponse;
import com.yuneec.droneservice.utils.ByteUtils;

import java.util.Arrays;

/**
 * Created by gjw on 2017/3/27.
 */

public class GetJoystickValue extends BaseCommand {

    public GetJoystickValue() {
        super(MAV_RC_COMMAND.MAV_RC_CMD_GET_HW_INPUT_VAL);
    }

    @Override
    public IResponse<JoystickValues> toResponse(byte[] data, int off, int len) {
        int code = data[off + 1] & 0xff;
        IResponse<JoystickValues> response = new IResponse(ids(), code);
        if (code == MAV_RC_ERROR_CODE.MAV_RC_ERR_SUCCESS) {
            JoystickValues joystickValues = new JoystickValues();
            int jCount = data[off + 2];
            int kCount = data[off + 3];
            int sCount = data[off + 4];
            int bCount = data[off + 5];
            if (jCount > 0) {
                int[] jValues = new int[jCount];
                for (int i = 0; i < jCount; i++) {
                    jValues[i] = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 6 + 2 * i, off + 6 + i * 2 + 2), IS_BIGENDIAN);
                }
                joystickValues.setjVaules(jValues);
            }
            if (kCount > 0) {
                int[] kValues = new int[kCount];
                for (int i = 0; i < kCount; i++) {
                    kValues[i] = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 6 + 2 * jCount + 2 * i, off + 6 + 2 * jCount + i * 2 + 2), IS_BIGENDIAN);
                }
                joystickValues.setkVaules(kValues);
            }
            if (sCount > 0) {
                int[] sValues = new int[sCount];
                for (int i = 0; i < sCount; i++) {
                    sValues[i] = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 6 + 2 * jCount + 2 * kCount + 2 * i, off + 6 + 2 * jCount + 2 * kCount + i * 2 + 2), IS_BIGENDIAN);
                }
                joystickValues.setsVaules(sValues);
            }
            if (bCount > 0) {
                int[] bValues = new int[bCount];
                for (int i = 0; i < bCount; i++) {
                    bValues[i] = ByteUtils.byteArrayToShort(Arrays.copyOfRange(data, off + 6 + 2 * jCount + 2 * kCount + 2 * sCount + 2 * i, off + 6 + 2 * jCount + 2 * kCount + 2 * sCount + i * 2 + 2), IS_BIGENDIAN);
                }
                joystickValues.setbVaules(bValues);
            }
            response.setParam(joystickValues);
        }
        return response;
    }

}
